﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace Astar
{
    public enum EvaluationFunctionType
    {
        Manhattan,
        Diagonal
    }
    public class Node
    {
        Vector2Int m_position;//下标
        public Vector2Int position => m_position;
        public Node parent;//上一个node

        //角色到该节点的实际距离
        int m_g;
        public int g
        {
            get => m_g;
            set
            {
                m_g = value;
                m_f = m_g + m_h;
            }
        }

        //该节点到目的地的估价距离
        int m_h;
        public int h
        {
            get => m_h;
            set
            {
                m_h = value;
                m_f = m_g + m_h;
            }
        }

        int m_f;
        public int f => m_f;

        public Node(Vector2Int pos, Node parent, int g, int h)
        {
            m_position = pos;
            this.parent = parent;
            m_g = g;
            m_h = h;
            m_f = m_g + m_h;
        }
    }
    /// <summary>
    /// 寻路地图网格信息
    /// 存储寻路所需的基本参数：起点、终点和地图障碍物信息
    /// </summary>
    public class SearchParameters
    {
        public Vector2Int startLocation { get; set; }  // 寻路起点位置
        public Vector2Int endLocation { get; set; }    // 寻路终点位置
        public bool[,] map { get; set; }            // 地图网格数据，true表示可通行，false表示障碍物

        /// <summary>
        /// 构造寻路参数对象
        /// </summary>
        /// <param name="startLocation">起点坐标</param>
        /// <param name="endLocation">终点坐标</param>
        /// <param name="map">地图网格数据，二维数组表示障碍物分布</param>
        public SearchParameters(Vector2Int startLocation, Vector2Int endLocation, bool[,] map)
        {
            this.startLocation = startLocation;
            this.endLocation = endLocation;
            this.map = map;
        }
        public override bool Equals(object obj)
        {
            if (obj == null) return false;
            if (obj is SearchParameters other)
            {
                if (other.startLocation == this.startLocation && other.endLocation == this.endLocation)
                    return true;
                else if(Vector2Int.Distance(other.startLocation,this.startLocation) < 4 && Vector2Int.Distance(other.endLocation,this.endLocation) < 4)
                    return true;
                else if(Vector2Int.Distance(other.startLocation,this.endLocation) < 2 && Vector2Int.Distance(other.endLocation,this.startLocation) < 8)
                    return true;
            }
            return false;
        }
        public override int GetHashCode()
        {
            return 1;
        }
    }

    public class AStarPath
    {
        static int FACTOR = 10;//水平竖直相邻格子的距离
        static int FACTOR_DIAGONAL = 14;//对角线相邻格子的距离

        bool m_isInit = false;
        public bool isInit => m_isInit;

        bool[,] m_map;//地图数据
        Vector2Int m_mapSize;
        Vector2Int m_start, m_destination;//起始点和结束点坐标
        EvaluationFunctionType m_evaluationFunctionType;//估价方式

        Dictionary<Vector2Int, Node> m_openDic = new Dictionary<Vector2Int, Node>();//准备处理的网格
        Dictionary<Vector2Int, Node> m_closeDic = new Dictionary<Vector2Int, Node>();//完成处理的网格

        Node m_destinationNode;
        public AStarPath(SearchParameters searchParameters)
        {
            this. m_map = searchParameters.map;
            this.m_start = searchParameters.startLocation;
            this.m_destination = searchParameters.endLocation;

            var x = m_map.GetLength(0);
            var y = m_map.GetLength(1);
            this.m_mapSize = new Vector2Int(x, y );

            Init();
        }
        void Init()
        {
            m_openDic.Clear();
            m_closeDic.Clear();

            AddNodeInOpenQueue(new Node(m_start, null, 0, 0));
            m_isInit = true;
            m_evaluationFunctionType = EvaluationFunctionType.Diagonal;
        }

        //计算寻路
        public IEnumerator Start(Action<List< Vector2Int>> callback)
        {
            if (!m_map[m_start.x, m_start.y] || !m_map[m_destination.x, m_destination.y])
            {
                Debug.LogError("错误的起点或终点");
                yield break;
            }

            // yield return new WaitForEndOfFrame();
            while (GridManager.instance.CurrentFinderCount > GridManager.MAX_FINDER_COUNT)
            {
                yield return null;
            }
            GridManager.instance.CurrentFinderCount++;

            //Debug.Log("开始寻路");
            while (m_openDic.Count > 0 && m_destinationNode == null)
            {
                //按照f的值升序排列
                m_openDic = m_openDic.OrderBy(kv => kv.Value.f).ToDictionary(p => p.Key, o => o.Value);
                //提取排序后的第一个节点
                Node node = m_openDic.First().Value;
                //因为使用的不是Queue，因此要从open中手动删除该节点
                m_openDic.Remove(node.position);
                //处理该节点相邻的节点
                OperateNeighborNode(node);
                //处理完后将该节点加入close中
                AddNodeInCloseDic(node);
                // yield return null;
            }
            yield return null;
            GridManager.instance.CurrentFinderCount--;
            if (m_destinationNode == null)
                Debug.LogError("找不到可用路径");
            else
                callback?.Invoke(ShowPath(m_destinationNode));
        }

        //处理相邻的节点
        void OperateNeighborNode(Node node)
        {
            for (int i = -1; i < 2; i++)
            {
                for (int j = -1; j < 2; j++)
                {
                    if (i == 0 && j == 0)
                        continue;
                    Vector2Int pos = new Vector2Int(node.position.x + i, node.position.y + j);
                    //超出地图范围
                    if (pos.x < 0 || pos.x >= m_mapSize.x || pos.y < 0 || pos.y >= m_mapSize.y)
                        continue;
                    //已经处理过的节点
                    if (m_closeDic.ContainsKey(pos))
                        continue;
                    //障碍物节点
                    if (m_map[pos.x, pos.y] == false)
                        continue;
                    //将相邻节点加入open中
                    if (i == 0 || j == 0)
                        AddNeighborNodeInQueue(node, pos, FACTOR);
                    else
                        AddNeighborNodeInQueue(node, pos, FACTOR_DIAGONAL);
                }
            }
        }

        //将节点加入到open中
        void AddNeighborNodeInQueue(Node parentNode, Vector2Int position, int g)
        {
            //当前节点的实际距离g等于上个节点的实际距离加上自己到上个节点的实际距离
            int nodeG = parentNode.g + g;
            //如果该位置的节点已经在open中
            if (m_openDic.ContainsKey(position))
            {
                //比较实际距离g的值，用更小的值替换
                if (nodeG < m_openDic[position].g)
                {
                    m_openDic[position].g = nodeG;
                    m_openDic[position].parent = parentNode;
                    //ShowOrUpdateAStarHint(m_openDic[position]);
                }
            }
            else
            {
                //生成新的节点并加入到open中
                Node node = new Node(position, parentNode, nodeG, GetH(position));
                //如果周边有一个是终点，那么说明已经找到了。
                if (position == m_destination)
                    m_destinationNode = node;
                else
                    AddNodeInOpenQueue(node);
            }
        }

        //加入open中，并更新网格状态
        void AddNodeInOpenQueue(Node node)
        {
            m_openDic[node.position] = node;
        }

        //加入close中，并更新网格状态
        void AddNodeInCloseDic(Node node)
        {
            m_closeDic.Add(node.position, node);
        }

        //寻路完成，显示路径
        List<Vector2Int> ShowPath(Node node)
        {
            //Debug.Log("寻路完成");
            List<Vector2Int> path = new List<Vector2Int>();
            while (node != null)
            {
                path.Add(node.position);
                //m_map[node.position.x, node.position.y].ChangeToPathState();
                node = node.parent;
            }
            path.Reverse();
            //Debug.Log("路径长度：" + path.Count);
            return path;
        }

        //获取估价距离
        int GetH(Vector2Int position)
        {
            if (m_evaluationFunctionType == EvaluationFunctionType.Manhattan)
                return GetManhattanDistance(position);
            else if (m_evaluationFunctionType == EvaluationFunctionType.Diagonal)
                return GetDiagonalDistance(position);
            else
                return Mathf.CeilToInt(GetEuclideanDistance(position));
        }

        //获取曼哈顿距离
        int GetDiagonalDistance(Vector2Int position)
        {
            int x = Mathf.Abs(m_destination.x - position.x);
            int y = Mathf.Abs(m_destination.y - position.y);
            int min = Mathf.Min(x, y);
            return min * FACTOR_DIAGONAL + Mathf.Abs(x - y) * FACTOR;
        }

        //获取对角线距离
        int GetManhattanDistance(Vector2Int position)
        {
            return Mathf.Abs(m_destination.x - position.x) * FACTOR + Mathf.Abs(m_destination.y - position.y) * FACTOR;
        }

        //获取欧几里得距离,测试下来并不合适
        float GetEuclideanDistance(Vector2Int position)
        {
            return Mathf.Sqrt(Mathf.Pow((m_destination.x - position.x) * FACTOR, 2) + Mathf.Pow((m_destination.y - position.y) * FACTOR, 2));
        }

        public void Clear()
        {
            m_closeDic.Clear();

            m_destinationNode = null;

            m_isInit = false;
        }
    }
}